from dobot_api import DobotApiDashboard, DobotApiMove, DobotApi, MyType
# from TCP import tcp
import numpy as np
import re
import json
import logging
current_actual = None
data = np.zeros((),dtype=MyType)
points = [350, 100, -90, 0]
from time import sleep,time
run_time_last = None
global a
a = 0
# 建立与机器人的连接
def ConnectRobot(ip:int,dashboardPort:int, movePort:int,feedPort:int):
    global move, dashboard, feed
    try:
        logging.info("Begin connect robot...")
        dashboard = DobotApiDashboard(ip, dashboardPort)
        move = DobotApiMove(ip, movePort)
        feed = DobotApi(ip, feedPort)
        logging.info("Connect robot success!")
        # return dashboard, move, feed
    except Exception as e:
        logging.error("connect robot failed!")
        raise e
# 使能机器人
def enable():
    logging.info("begin to enable robot...")
    dashboard.EnableRobot()
    DobotApiMove.MovL(move, 200, 0, 0, 0)
    logging.info("robot enable done")
    return "使能完成"
def disable():
    logging.info("begin to disable robot...")
    dashboard.DisableRobot()
    logging.info("robot disable done")
def move_set(speed:int,accj:int,accl:int,speedj:int,speedl:int,cp:int):
    DobotApiDashboard.SpeedFactor(dashboard,speed)
    DobotApiDashboard.AccJ(dashboard,accj)
    DobotApiDashboard.AccL(dashboard,accl)
    DobotApiDashboard.SpeedJ(dashboard,speedj)
    DobotApiDashboard.SpeedL(dashboard,speedl)
    DobotApiDashboard.CP(dashboard,cp)
def chip_abandon(location:list):
    # location[1] += 3.5
    # location[2] -= 4
    location[3] = -location[3]
    location.insert(3, -102.8)  # 在位置列表的第三个位置插入一个值
    location_first = location.copy()
    location_first[3] += 15
    try:
        # 执行动作序列

        DobotApiMove.MovJ(move,location_first[1],location_first[2],location_first[3],location_first[4])
        DobotApiMove.MovJ(move, location[1], location[2], location[3], location[4])  # 线性移动至指定位置
        DobotApiDashboard.DO(dashboard, 1, 1)  # 执行数字输出2，开启某个功能
        DobotApiMove.RelMovL(move, 0, 0, 30, 0)  # 相对位置移动
        DobotApiMove.MovJ(move, points[0], points[1], points[2]+20, points[3])  # 再次线性移动
        DobotApiMove.MovJ(move, points[0], points[1], points[2], points[3])  # 最后一次线性移动
        DobotApiDashboard.DO(dashboard, 1, 0)  # 关闭数字输出2
        DobotApiDashboard.DO(dashboard, 2, 1)  # 打开数字输出1
        DobotApiMove.RelMovL(move, 0, 0, 30, 0)  # 相对位置移动
        DobotApiDashboard.DO(dashboard, 2, 0)  # 关闭数字输出1
        points[2]+=5

        wait(points[0], points[1])
        sleep(2)
        # run()
        # DobotApiMove.MovJ(move,200,0,0,0)
        # run()

    except Exception as e:
        return (f"运行时出现异常: {str(e)}")

# 运行指定动作序列
def chip_place(num: int, location: list):
    global dashboard, move
    # 定义动作序列的位置映射
    maps = {
        0: [377, -111, -93, 0],
        1: [350, -111.5, -94.5, 0],
        2: [327, -109, -93, 0],
        3: [375.5, -139.5, -97, 0],
        4: [351.5, -141.5, -97, 0],
        5: [326, -139.5, -94.5, 0],
        9: [402, -27, -101, 0],
    }
    point = maps.get(num, [0, 0, 0, 0])  # 获取对应动作的目标位置，默认为零点

    # location[1]+=8
    # location[2]-=12
    location[3] = -location[3]
    location.insert(3, -103.8)  # 在位置列表的第三个位置插入一个值
    location_first = location.copy()
    location_first[3] += 15
    try:
        # 执行动作序列
        DobotApiMove.MovJ(move,location_first[1],location_first[2],location_first[3],location_first[4])
        DobotApiMove.MovJ(move, location[1], location[2], location[3], location[4])  # 线性移动至指定位置
        DobotApiDashboard.wait(dashboard,200)
        DobotApiDashboard.DO(dashboard, 1, 1)  # 执行数字输出2，开启某个功能
        DobotApiDashboard.wait(dashboard,200)
        if location[0] == 9:
            DobotApiMove.MovJ(move, point[0], point[1], point[2], point[3])
        else:
            DobotApiMove.MovJ(move, location_first[1], location_first[2], location_first[3], location_first[4])
            DobotApiMove.MovJ(move, point[0], point[1], location_first[3], point[3])  # 再次线性移动
            DobotApiMove.MovJ(move, point[0], point[1], point[2], point[3])  # 最后一次线性移动
        DobotApiDashboard.wait(dashboard,50)
        DobotApiDashboard.DO(dashboard, 1, 0)  # 关闭数字输出2
        DobotApiDashboard.DO(dashboard, 2, 1)  # 打开数字输出1
        if location[0] == 9:
            DobotApiMove.MovJ(move, 200, 0, 0, 0)
            DobotApiDashboard.DO(dashboard, 2, 0)  # 关闭数字输出1
            # sleep(1)
            sleep(2)
            # run()
        else:
            DobotApiMove.RelMovL(move, 0, 0, 30, 0)  # 相对位置移动
            DobotApiDashboard.DO(dashboard, 2, 0)  # 关闭数字输出1
            # sleep(1)
            wait(point[0],point[1])
            # run()

    except Exception as e:
        print(f"运行时出现异常: {str(e)}")

#相对运动函数
def  move_rel(x:float,y:float,z:float):
    DobotApiMove.RelMovL(move,x,y,z)

def move_abs(x:float,y:float,z:float):
    DobotApiMove.MovJ(move,x,y,z)
# def run():
#     tcp.send('1')
#     location = tcp.recv()
#     if location[0] == 8:
#         chip_abandon(location)
#     else:
#         chip_place(location[0], location)  # 执行动作序列

def wait(x:int,y:int):
    while True:
            str1 = DobotApiDashboard.GetPose(dashboard)
            point2 = re.search(r"\{(.*?)\}", str1).group(1)
            point2 = json.loads(f'[{point2}]')
            if point2[0]-x<=1 and point2[1]-y<=1:
                break
            sleep(0.9)

# 主函数
def main():
    # tcp.enable('127.0.0.1',12345)
    enable("192.168.1.6", 29999, 30003, 30004)  # 连接并使能机器人
    move_set(100, 100, 100, 100, 100, 1)
    # run()
# 主程序入口
if __name__ == '__main__':
    main()
    # while True:
    #     sleep(0.5)
    #     str1 = DobotApiDashboard.GetPose(dashboard)
    #     point2 = re.search(r"\{(.*?)\}", str1).group(1)
    #     point2 = json.loads(f'[{point2}]')
    #     print('333333333333333333333333333333333')
    #     print(point2[1])
    #     run_time = time()
    #     if run_time_last == None:
    #         run_time_last = run_time
    #     time_diff = run_time_last - run_time
    #     print(time_diff)
    #     while ((90 < abs(point2[1]) or  point2[1] < -75)and time_diff<2):
    #         a =0
    #     run()

